/*!
  @file marker_generator.cpp
  @brief Brief description
  @author Kyohei Otsu <kyohei@kth.se>
  @date 2013
*/

#include <geometry_msgs/Point.h>

#include "mmo_vo/marker_generator.h"

namespace vo
{

MarkerGenerator::MarkerGenerator()
{
  cur_id_ = 0;
}

MarkerGenerator::~MarkerGenerator()
{
}

visualization_msgs::Marker::Ptr MarkerGenerator::generate_feature_pts(std::vector<cv::KeyPoint> &kp)
{
  visualization_msgs::Marker::Ptr marker(new visualization_msgs::Marker);
  // marker->header should be filled by the caller

  marker->ns = "visualization";
  marker->id = cur_id_++;
  marker->type = visualization_msgs::Marker::POINTS; // point
  marker->action = visualization_msgs::Marker::ADD;

  marker->pose.position.x = 0.0;
  marker->pose.position.y = 0.0;
  marker->pose.position.z = 0.0;
  marker->pose.orientation.x = 0.0;
  marker->pose.orientation.y = 0.0;
  marker->pose.orientation.z = 0.0;
  marker->pose.orientation.w = 1.0;

  marker->scale.x = 1.0;
  marker->scale.y = 1.0;
  marker->scale.z = 1.0;

  marker->color.a = 1.0;
  marker->color.r = 1.0;
  marker->color.g = 0.0;
  marker->color.b = 0.0;

  geometry_msgs::Point pt;
  std::vector<cv::KeyPoint>::iterator itr = kp.begin();
  while(itr != kp.end())
  {
    pt.x = itr->pt.x;
    pt.y = itr->pt.y;
    pt.z = 0.0;
    marker->points.push_back(pt);
    itr++;
  }

  return marker;
}


} // end of namespace
